function [] = vld_kalmfilt_e(in)
% 蒙塔卡罗验证卡尔曼滤波器基本估计能力

%% 常量
par.g = 0; % 重力加速度为0
cst = load('earthconstants_virtual.mat', 'RE');
cst.deg2Rad = pi / 180;
cst.sec2Rad = pi / 3600 / 180;

%% 生成轨迹
% 常量
cst_trajGen = load('earthconstants_virtual.mat', 'omegaIE', 'RE', 'epsilonSq');
% 配置
cfg_trajGen.enableTrajectoryGen = true;
cfg_trajGen.enableSensErr = true; % 在轨迹中引入器件误差
cfg_trajGen.enableSensRandErr = true; % 在轨迹中引入器件随机误差
cfg_trajGen.enableNavigation = false;
cfg_trajGen.ignore2OdSensErr = true;
cfg_trajGen.IMUOutFilePath = [];
cfg_trajGen.quantizeIMUOut = false;
cfg_trajGen.IMUOutPrecision = '%.16e';
% 参量
par_trajGen.samplePeriod = 0.01;
par_trajGen.g = par.g; % 重力加速度，m/s^2
par_trajGen.IMU.acc.B = in.curSimParVal(10:12, 1) * 9.8 * 1e-6; % 转换前单位μg，转换后单位m/s^2
par_trajGen.IMU.acc.SF0 = ones(3, 1);
par_trajGen.IMU.acc.dSF = in.curSimParVal(13:15, 1) * 1e-6;
par_trajGen.IMU.acc.MA = vec2offdiag(in.curSimParVal(16:21, 1)) * 1e-6; % 转换前单位μrad，转换后单位rad
par_trajGen.IMU.acc.SO = in.curSimParVal(22:24, 1) * 1e-6 / 9.8; % 转换前单位μg/g^2，转换后单位1/(m/s^2)
par_trajGen.IMU.acc.R.K = in.curSimParVal(38, 1) * ones(3, 1) * (9.8/1e6) / 60; % 转换前单位μg/sqrt(h)，转换后单位m/s^(5/2)
par_trajGen.IMU.acc.R.N = in.curSimParVal(39, 1) * ones(3, 1) * (9.8/1e6) * 60; % 转换前单位μg*sqrt(h)，转换后单位m/s^(3/2)
par_trajGen.IMU.gyro.B = in.curSimParVal(25:27, 1) * cst.sec2Rad; % 转换前单位°/h，转换后单位rad/s
par_trajGen.IMU.gyro.SF0 = ones(3, 1);
par_trajGen.IMU.gyro.dSFp = in.curSimParVal(28:30, 1) * 1e-6;
par_trajGen.IMU.gyro.dSFn = par_trajGen.IMU.gyro.dSFp;
par_trajGen.IMU.gyro.MA = vec2offdiag(in.curSimParVal(31:36, 1)) * 1e-6; % 转换前单位μrad，转换后单位rad
par_trajGen.IMU.gyro.GS = zeros(3);
par_trajGen.IMU.gyro.R.K = in.curSimParVal(42, 1) * ones(3, 1) * cst.deg2Rad / (3600^(3/2)); % 转换前单位°/h^(3/2)，转换后单位rad/s^(3/2)
par_trajGen.IMU.gyro.R.N = in.curSimParVal(43, 1) * ones(3, 1) * cst.deg2Rad / 60; % 转换前单位°/sqrt(h)，转换后单位rad/sqrt(s)
% 输入量
in_trajGen.genFuncName = 'gentgi_filtervld';
in_trajGen.genFuncInArg = {};
% 初始条件
iniCon_trajGen.CBG = eye(3); % 初始姿态矩阵
iniCon_trajGen.vG = zeros(3, 1); % 初始速度
iniCon_trajGen.pos = [30 * cst.deg2Rad, 0 * cst.deg2Rad, 0]'; % 纬度、经度、高度 % TODO: 改为0纬度
% 生成轨迹
[out_trajGen, ~] = gentrajectory(in_trajGen, cfg_trajGen, par_trajGen, iniCon_trajGen, cst_trajGen);

%% 运行组合导航
% 常量
cst_intNav = load('earthconstants_virtual.mat', 'RE', 'e', 'mu', 'J2', 'J3', 'omegaIE', 'epsilonSq');
% 配置
cfg_intNav.enableKalmFilt = false;
cfg_intNav.enableKFUpdate = false;
cfg_intNav.extrapAlgo = KalmFiltExtrapAlgo.ITER;
cfg_intNav.updAlgo = KalmFiltUpdAlgo.JOSEPH;
cfg_intNav.useDCMInAttCalc = false;
cfg_intNav.dynaMatINSSysBlkForm = 1;
cfg_intNav.navCoordType = NavCoordType.FREE_AZM; % TODO: 默认为WAN_AZM
cfg_intNav.sysStateMask = [true(1, 10) false(1, 1)];
cfg_intNav.sensStateMask = [true(1, 27) false(1, 9)];
cfg_intNav.enableNumCondCtrl = false; % 关闭数值条件控制
% 参量
par_intNav.tBgn = out_trajGen.t(1);
par_intNav.tEnd = out_trajGen.t(end);
par_intNav.dT = par_trajGen.samplePeriod;
par_intNav.tUpd = 1; % Aid组合导航的量测更新周期
par_intNav.g = par_trajGen.g;
par_intNav.INSCycPerKalFiltExtrapCyc = int32(10);
par_intNav.GammaMRGammaMT = diag([2/cst.RE, 2/cst.RE, 2, 0.005, 0.005, 0.005, 10*cst.sec2Rad*ones(1, 3), 2/cst.RE].^2);
par_intNav.INSSensRWC = [par_trajGen.IMU.acc.R.N; par_trajGen.IMU.gyro.R.N];
par_intNav.INSSensStateProcNoisePSDFull = [par_trajGen.IMU.acc.R.K.^2; zeros(12, 1); par_trajGen.IMU.gyro.R.K.^2; zeros(18, 1)];
par_intNav.PDiagMin = NaN(nnz([cfg_intNav.sysStateMask cfg_intNav.sensStateMask]), 1);
par_intNav.PDiagMax = NaN(nnz([cfg_intNav.sysStateMask cfg_intNav.sensStateMask]), 1);
% 初始条件
iniCon_intNav.pos = out_trajGen.pos(1, :)'; % 无误差
iniCon_intNav.vN = out_trajGen.vG(1, :)'; % 无误差
iniCon_intNav.CBN = out_trajGen.CBG(:, :, 1); % 无误差
iniCon_intNav.kalmFilt.x = [-in.parRefVal(1, 2)/cst.RE; in.parRefVal(2, 2)/cst.RE; in.parRefVal(3, 2); in.parRefVal(6:-1:4, 2); in.parRefVal(9:-1:7, 2)*cst.sec2Rad; in.parRefVal(2, 2)/cst.RE*tan(out_trajGen.pos(1, 1));...
    in.parRefVal(10:12, 2)*9.8*1e-6; in.parRefVal(13:21, 2)*1e-6; in.parRefVal(22:24, 2)/9.8*1e-6; in.parRefVal(25:27, 2)*cst.sec2Rad; in.parRefVal(28:36, 2)*1e-6];
iniCon_intNav.kalmFilt.P = diag(iniCon_intNav.kalmFilt.x.^2);
aidMeasNoise = sqrt(diag(par_intNav.GammaMRGammaMT))';
% 输入量
m = size(out_trajGen.vG, 1);
in_intNav.INS.accOut = out_trajGen.accOutErrFree;
in_intNav.INS.gyroOut = out_trajGen.gyroOutErrFree;
in_intNav.aid.CNE = zeros(3, 3, m);
in_intNav.aid.vN = zeros(m, 3);
in_intNav.aid.vG = out_trajGen.vG;
in_intNav.aid.CBN = zeros(3, 3, m);
in_intNav.aid.h = out_trajGen.pos(:, 3);
in_intNav.aid.alpha = zeros(m, 1);
in_intNav.traj.CNE = zeros(3, 3, m);
in_intNav.traj.pos = out_trajGen.pos;
in_intNav.traj.vN = out_trajGen.vG;
in_intNav.traj.CBN = zeros(3, 3, m);
in_intNav.traj.fB = out_trajGen.fBTrue;
in_intNav.traj.omegaIBB = out_trajGen.omegaIBBTrue;
in_intNav.traj.FCN = zeros(3, 3, m);
in_intNav.traj.gN = zeros(m, 3);
in_intNav.u = NaN(0, 1);
% 运行导航
clear('intnav_kfvld');
% 纯惯导解算(TODO: 因为轨迹发生器无vN, CNE, CBN, FCN, gN, alpha参数输出，故利用纯惯导解算获取这些参数，后续待轨迹发生器完善之后可删除此段纯惯导解算代码)
[out_inertNavIdeal, ~] = intnav_kfvld(in_intNav, cfg_intNav, par_intNav, iniCon_intNav, cst_intNav);
% 组合导航
cfg_intNav.enableKalmFilt = true;
cfg_intNav.enableKFUpdate = true;
iniCon_intNav.kalmFilt.x = zeros(37, 1); % 初始状态为0
[dL, dlambda, ~] = dposvec2dllwa(in.curSimParVal(2, 1), in.curSimParVal(1, 1), NaN, out_trajGen.pos(1, 1), out_inertNavIdeal.alpha(1, 1), out_inertNavIdeal.CNE(:, :, 1), out_trajGen.pos(1, 3), cst_intNav.RE, cst_intNav.e);
iniCon_intNav.pos = out_trajGen.pos(1, :)' + [dL; dlambda; in.curSimParVal(3, 1)];
iniCon_intNav.vN = out_trajGen.vG(1, :)' + in.curSimParVal(6:-1:4, 1);
iniCon_intNav.CBN = angle2dcm(in.curSimParVal(7)*cst.sec2Rad, in.curSimParVal(8)*cst.sec2Rad, in.curSimParVal(9)*cst.sec2Rad) * out_trajGen.CBG(:, :, 1);
in_intNav.INS.accOut = out_trajGen.accOut;
in_intNav.INS.gyroOut = out_trajGen.gyroOut;
in_intNav.aid.h = out_trajGen.pos(:, 3) + aidMeasNoise(1, 3)*randn(m, 1);
in_intNav.aid.vN = out_inertNavIdeal.vN + repmat(aidMeasNoise(1, 4:6), m, 1).*randn(m, 3);
in_intNav.aid.vG = out_trajGen.vG + repmat(aidMeasNoise(1, 4:6), m, 1).*randn(m, 3);
for i=1:m
    in_intNav.aid.CNE(:, :, i) = out_inertNavIdeal.CNE(:, :, i) * angle2dcm(aidMeasNoise(1, 10)*randn, aidMeasNoise(1, 2)*randn, aidMeasNoise(1, 1)*randn);
    in_intNav.aid.CBN(:, :, i) = out_inertNavIdeal.CBN(:, :, i) * angle2dcm(aidMeasNoise(1, 9)*randn, aidMeasNoise(1, 8)*randn, aidMeasNoise(1, 7)*randn);
end
in_intNav.aid.alpha = out_inertNavIdeal.alpha;
in_intNav.traj.CNE = out_inertNavIdeal.CNE;
in_intNav.traj.CBN = out_inertNavIdeal.CBN;
in_intNav.traj.fB = out_trajGen.fBTrue;
in_intNav.traj.omegaIBB = out_trajGen.omegaIBBTrue;
in_intNav.traj.FCN = out_inertNavIdeal.FCN;
in_intNav.traj.gN = out_inertNavIdeal.gN;
[out_intNavError, auxOut_intNavError] = intnav_kfvld(in_intNav, cfg_intNav, par_intNav, iniCon_intNav, cst_intNav); %#ok<NASGU,ASGLU>

%% 保存结果
if ~isdir([in.CWD '\data\'])
    mkdir([in.CWD '\data\']);
end
tmp_sim.dataPath = [in.CWD '\data\'];
tmp_sim.postfix = ['_' int2str(in.curGrpIdx) '_' int2str(in.curSimIdx)];
save([tmp_sim.dataPath '导航结果' tmp_sim.postfix], 'out_intNavError', 'auxOut_intNavError', 'out_inertNavIdeal', 'par_trajGen', 'cst', 'iniCon_trajGen', 'par_intNav', 'iniCon_intNav');
end